#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/timers.h"
#include "freertos/queue.h"
#include "freertos/list.h"
#include "esp_check.h"
#include "motor_control/controller.h"

typedef struct mc_t {
    bdc_motor_handle_t motor;
    encoder_handle_t encoder;
    pid_ctrl_block_handle_t pid;
    ListItem_t item;
    float max_rpm;
    float expect_rpm;
    pid_ctrl_parameter_t pid_params;
} mc_t;

#define TAG "mc"

#define RETURN_ON_FALSE(a, msg) ESP_RETURN_ON_FALSE(a, ESP_ERR_INVALID_STATE, TAG, msg)
#define RETURN_ON_INCREATED(msg) RETURN_ON_FALSE(g_timer_handle == NULL, msg)
#define RETURN_ON_UNCREATED(msg) RETURN_ON_FALSE(g_timer_handle != NULL, msg)
#define RETURN_ON_INACTIVE(msg) RETURN_ON_FALSE(!xTimerIsTimerActive(g_timer_handle), msg)
#define RETURN_ON_UNACTIVE(msg) RETURN_ON_FALSE(xTimerIsTimerActive(g_timer_handle), msg)
#define RETURN_ON_LIST_EMPTY(msg) RETURN_ON_FALSE(!listLIST_IS_EMPTY(&g_list), msg)
#define RETURN_ON_LIST_NOT_EMPTY(msg) RETURN_ON_FALSE(listLIST_IS_EMPTY(&g_list), msg)
#define RETURN_ON_ENCODER_IN_LIST(handle, msg) RETURN_ON_FALSE(!listIS_CONTAINED_WITHIN(&g_list, &handle->item), msg)
#define RETURN_ON_ENCODER_NOT_IN_LIST(handle, msg) RETURN_ON_FALSE(listIS_CONTAINED_WITHIN(&g_list, &handle->item), msg)

static List_t g_list;
static TimerHandle_t g_timer_handle = NULL;

static void pid_loop_tcb(TimerHandle_t xTimer) {
    ListItem_t *item = listGET_HEAD_ENTRY(&g_list);
    while (item != listGET_END_MARKER(&g_list)) {
        mc_handle_t handle = item->pvOwner;
        float error = handle->expect_rpm - encoder_get_rpm(handle->encoder);
        float new_duty;
        pid_compute(handle->pid, error, &new_duty);
        if (new_duty > 0) {
            bdc_motor_forward(handle->motor);
            bdc_motor_set_speed(handle->motor, new_duty);
        } else {
            bdc_motor_reverse(handle->motor);
            bdc_motor_set_speed(handle->motor, -new_duty);
        }
        item = listGET_NEXT(item);
    }
}

static esp_err_t mc_start(mc_handle_t handle) {
    ESP_LOGD(TAG, "Enable motor");
    ESP_RETURN_ON_ERROR(bdc_motor_enable(handle->motor), TAG, "enable motor failed");
    
    ESP_LOGD(TAG, "Forward motor");
    ESP_RETURN_ON_ERROR(bdc_motor_forward(handle->motor), TAG, "forward motor failed");

    return ESP_OK;
}

static esp_err_t mc_stop(mc_handle_t handle) {
    ESP_LOGD(TAG, "disable motor unit");
    ESP_RETURN_ON_ERROR(bdc_motor_disable(handle->motor), TAG, "disable motro failed");

    return ESP_OK;
}

esp_err_t mc_timer_loop_create(TickType_t pid_period_tick, TickType_t encoder_period_tick) {
    RETURN_ON_INCREATED("timer loop has been already created");

    esp_err_t ret;

    ESP_RETURN_ON_ERROR(encoder_timer_loop_create(encoder_period_tick), TAG, "");

    g_timer_handle = xTimerCreate("pid_loop",
        pid_period_tick, pdTRUE, NULL, pid_loop_tcb);
    ESP_GOTO_ON_FALSE(g_timer_handle != NULL, ESP_ERR_NO_MEM, err, TAG, "create timer loop failed");

    vListInitialise(&g_list);

    return ESP_OK;

err:
    ESP_RETURN_ON_ERROR(encoder_timer_loop_del(), TAG, "");
    return ret;
}

esp_err_t mc_timer_loop_del(void) {
    RETURN_ON_UNCREATED("timer loop is not created or has been deleted");
    RETURN_ON_INACTIVE("please call `mc_timer_loop_stop` to stop it at first");
    RETURN_ON_LIST_NOT_EMPTY("please delete all encoder handle at first");

    ESP_RETURN_ON_ERROR(encoder_timer_loop_del(), TAG, "");
    ESP_RETURN_ON_FALSE(xTimerDelete(g_timer_handle, 0), ESP_FAIL, TAG, "stop timer loop failed");
    g_timer_handle = NULL;
    return ESP_OK;
}

esp_err_t mc_timer_loop_start(void) {
    RETURN_ON_UNCREATED("please call `mc_timer_loop_init` to create timer loop at first");
    RETURN_ON_INACTIVE("timer loop has been already started");

    ESP_RETURN_ON_ERROR(encoder_timer_loop_start(), TAG, "");

    esp_err_t ret;
    ListItem_t *item = listGET_HEAD_ENTRY(&g_list);
    ListItem_t *_item = item;
    while (item != listGET_END_MARKER(&g_list)) {
        mc_handle_t handle = item->pvOwner;
        ESP_GOTO_ON_ERROR(mc_start(handle), err, TAG, "start failed");
        item = listGET_NEXT(item);
    }

    ESP_GOTO_ON_FALSE(xTimerStart(g_timer_handle, 0), ESP_FAIL, err, TAG, "start timer loop failed");

    return ESP_OK;

err:
    ESP_RETURN_ON_ERROR(encoder_timer_loop_stop(), TAG, "");
    while (_item != item) {
        mc_handle_t handle = item->pvOwner;
        ESP_RETURN_ON_ERROR(mc_stop(handle), TAG, "stop failed");
        item = listGET_NEXT(item);
    }

    return ret;
}

esp_err_t mc_timer_loop_stop(void) {
    RETURN_ON_UNCREATED("timer loop is not created");
    RETURN_ON_UNACTIVE("timer loop is not started or has been stopped");

    ESP_RETURN_ON_FALSE(xTimerStop(g_timer_handle, 0), ESP_FAIL, TAG,
                        "deactivate timer loop failed");

    ListItem_t *item = listGET_HEAD_ENTRY(&g_list);
    while (item != listGET_END_MARKER(&g_list)) {
        mc_handle_t handle = item->pvOwner;
        ESP_RETURN_ON_ERROR(mc_stop(handle), TAG, "stop failed");
        item = listGET_NEXT(item);
    }

    ESP_RETURN_ON_ERROR(encoder_timer_loop_stop(), TAG, "");

    return ESP_OK;
}

esp_err_t mc_new(const mc_config_t *config, float max_rpm, mc_handle_t *ret_handle) {
    esp_err_t ret;
    ESP_RETURN_ON_FALSE(config != NULL && ret_handle != NULL, ESP_ERR_NO_MEM,
        TAG, "no mem for motor control handle");
    RETURN_ON_UNCREATED("timer loop is not created");
    RETURN_ON_INACTIVE("can not create encoder when timer loop is active");

    mc_handle_t handle = malloc(sizeof(mc_t));
    ESP_RETURN_ON_FALSE(handle != NULL, ESP_ERR_NO_MEM, TAG, "no mem for motor control handle");

    ESP_GOTO_ON_ERROR(bdc_motor_new_mcpwm_device(
        &config->motor_config, &config->mcpwm_config, &handle->motor), err,
        TAG, "create mcpwm device failed");

    ESP_GOTO_ON_ERROR(encoder_new(&config->encoder_config, &handle->encoder), err,
        TAG, "create encoder handle failed");

    ESP_GOTO_ON_ERROR(pid_new_control_block(&config->pid_config, &handle->pid), err,
        TAG, "create pid control block failed");
    
    ESP_RETURN_ON_ERROR(encoder_add_to_timer_loop(handle->encoder), "TAG", "");

    handle->expect_rpm = 0;
    handle->max_rpm = max_rpm;
    vListInitialiseItem(&handle->item);
    handle->item.pvOwner = handle;

    handle->pid_params = config->pid_config.init_param;

    *ret_handle = handle;
    return ESP_OK;

err:
    return ret;
}

esp_err_t mc_del(mc_handle_t handle) {
    RETURN_ON_ENCODER_IN_LIST(handle, "please remove encoder from timer loop at first");
    
    ESP_RETURN_ON_ERROR(encoder_remove_from_time_loop(handle->encoder), "TAG", "");

    if (handle->motor) {
        ESP_RETURN_ON_ERROR(bdc_motor_del(handle->motor), TAG, "delete bdc motor failed");
        handle->motor = NULL;
    }

    if (handle->encoder) {
        ESP_RETURN_ON_ERROR(encoder_del(handle->encoder), TAG, "delete encoder failed");
        handle->encoder = NULL;
    }

    if (handle->pid) {
        ESP_RETURN_ON_ERROR(pid_del_control_block(handle->pid), TAG, "delete pid control block failed");
        handle->pid = NULL;
    }

    free(handle);

    return ESP_OK;
}

esp_err_t mc_add_to_timer_loop(mc_handle_t handle) {
    RETURN_ON_UNCREATED("timer loop is not created");
    RETURN_ON_INACTIVE("the function can be call if and only if timer loop is not active");
    RETURN_ON_ENCODER_IN_LIST(handle, "it is already in list");

    vListInsertEnd(&g_list, &handle->item);

    return ESP_OK;
}

esp_err_t mc_remove_from_timer_loop(mc_handle_t handle) {
    RETURN_ON_UNCREATED("timer loop is not created");
    RETURN_ON_INACTIVE("the function can be call if and only if timer loop is not active");
    RETURN_ON_ENCODER_NOT_IN_LIST(handle, "it is not in list");

    uxListRemove(&handle->item);

    return ESP_OK;
}

void mc_set_pid_params(mc_handle_t handle, float kp, float ki, float kd) {
    handle->pid_params.kp = kp;
    handle->pid_params.ki = ki;
    handle->pid_params.kd = kd;
    pid_update_parameters(handle->pid, &handle->pid_params);
}

void mc_set_expect_rpm(mc_handle_t handle, float expect_rpm) {
    handle->expect_rpm = expect_rpm > handle->max_rpm ? handle->max_rpm
        : expect_rpm < -handle->max_rpm ? -handle->max_rpm : expect_rpm;
}

float mc_get_real_rpm(mc_handle_t handle) {
    return encoder_get_rpm(handle->encoder);
}
